/*
$GNGGA,,,,,,0,00,25.5,,,,,,*64
$GNGLL,,,,,,V,N*7A
$GPGSA,A,1,,,,,,,,,,,,,25.5,25.5,25.5*02
$BDGSA,A,1,,,,,,,,,,,,,25.5,25.5,25.5*13
$GPGSV,1,1,00*79
$BDGSV,1,1,00*68
$GNRMC,,V,,,,,,,,,,N*4D
$GNVTG,,,,,,,,,N*2E
$GNZDA,,,,,,*56
$GPTXT,01,01,01,ANTENNA OPEN*25
*/
#include "sle_low_latency.h"

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <sys/time.h>
#include "cmsis_os2.h"
#include "app_init.h"
#include "soc_osal.h"

#include "common_def.h"
#include "pinctrl.h"
#include "uart.h"
#include "gps.h"
#include "securec.h"

#include "lwip/nettool/misc.h"
#include "lwip/ip4_addr.h"
#include "lwip/netif.h"
#include "lwip/netifapi.h"
#include "lwip/sockets.h"

#include "watchdog.h"
#include "osal_debug.h"

#include "gpio.h"

#define GPS_BAUDRATE                      9600
#define GPS_DATA_BITS                     3
#define GPS_STOP_BITS                     1
#define GPS_PARITY_BIT                    0
#define GPS_TRANSFER_SIZE                 2048  // 增大缓冲区以容纳完整NMEA句子

#define GPS_TASK_STACK_SIZE               0x1000
#define GPS_TASK_PRIO                     (osPriority_t)(17)
#define GPS_TASK_DURATION_MS              5000

static uint8_t g_app_uart_rx_buff[GPS_TRANSFER_SIZE] = { 0 };
static char nmea_buffer[GPS_TRANSFER_SIZE] = {0};

static size_t nmea_buffer_pos = 0;
static GPS_Data gps_data = {0};
char gngga[45]={0};
char weidu[15]={0};
char jdu[15]={0};
char haiba[5]={0};
char staflag;
bool gpsflag=true;
bool openGpsFlag = true;
bool ledFlag = true;
// bool ledFlagRad = false;
static uart_buffer_config_t g_app_uart_buffer_config = {
    .rx_buffer = g_app_uart_rx_buff,
    .rx_buffer_size = GPS_TRANSFER_SIZE
};

static void app_gps_init(void)
{
    uapi_pin_set_mode(CONFIG_GPS_TXD_PIN, PIN_MODE_1);
    uapi_pin_set_mode(CONFIG_GPS_RXD_PIN, PIN_MODE_1);

    uart_attr_t attr = {
        .baud_rate = GPS_BAUDRATE,
        .data_bits = GPS_DATA_BITS,
        .stop_bits = GPS_STOP_BITS,
        .parity = GPS_PARITY_BIT
    };

    uart_pin_config_t pin_config = {
        .tx_pin = CONFIG_GPS_TXD_PIN,
        .rx_pin = CONFIG_GPS_RXD_PIN,
        .cts_pin = PIN_NONE,
        .rts_pin = PIN_NONE
    };
    uapi_uart_deinit(CONFIG_GPS_BUS_ID); 
    uapi_uart_init(CONFIG_GPS_BUS_ID, &pin_config, &attr, NULL, &g_app_uart_buffer_config);
}








// 解析GGA句子
static void parse_gga(const char *sentence) {
    char status;
    int satellites;
    float hdop, altitude;
    char alt_unit, lat_dir, lon_dir;
    float lat_deg, lon_deg;
    char time[12];
    char lat[16], lon[16];
    
    osal_printk("Raw GGA: %s\n", sentence);
    
    // 示例格式: $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
    int parsed = sscanf(sentence, "$G%*CGGA,%11[^,],%15[^,],%c,%15[^,],%c,%c,%d,%f,%f,%c,%*f,%c",
                       time, lat, &lat_dir, lon, &lon_dir, &status, &satellites, &hdop, &altitude, &alt_unit, &alt_unit);
    
    // osal_printk("Parsed GGA fields: %d\n", parsed);
    
    if (parsed >= 9) {
        // 解析时间
        if (strlen(time) >= 6U) {
            snprintf(gps_data.time, sizeof(gps_data.time), "%.2s:%.2s:%.2s", time, time+2, time+4);
            // osal_printk("GGA Time: %s\n", gps_data.time);
        }
        
        // 解析纬度
        if (strlen(lat) >= 4U && lat_dir) {
            sscanf(lat, "%f", &lat_deg);
            int degrees = (int)(lat_deg / 100);
            float minutes = lat_deg - degrees * 100;
            gps_data.latitude = degrees + minutes / 60.0;
            if (lat_dir == 'S') gps_data.latitude = -gps_data.latitude;
            // osal_printk("GGA Latitude: %f %c\n", gps_data.latitude, lat_dir);
        }
        
        // 解析经度
        if (strlen(lon) >= 5U && lon_dir) {
            sscanf(lon, "%f", &lon_deg);
            int degrees = (int)(lon_deg / 100);
            float minutes = lon_deg - degrees * 100;
            gps_data.longitude = degrees + minutes / 60.0;
            if (lon_dir == 'W') gps_data.longitude = -gps_data.longitude;
            // osal_printk("GGA Longitude: %f %c\n", gps_data.longitude, lon_dir);
        }
        
        gps_data.status = status;
        gps_data.satellites = satellites;
        gps_data.hdop = hdop;
        gps_data.altitude = altitude;
        snprintf(gngga, 45, "%lf,%lf,%f,%c", 
                gps_data.latitude, 
                gps_data.longitude, 
                gps_data.altitude,
                gps_data.status);
        snprintf(weidu,15,"%.6f",gps_data.latitude);
        snprintf(jdu,15,"%.6f",gps_data.longitude);
        snprintf(haiba,5,"%.1f",gps_data.altitude);
        // snprintf(staflag,1,"%.6f",gps_data.latitude);
        staflag=gps_data.status;
        // osal_printk("GGA Status: %c, Satellites: %d, HDOP: %.1f, Altitude: %.1f %c\n",
                //    status, satellites, hdop, altitude, alt_unit);
    } else {
        strncpy(gngga, "0.000000,0.000000,0.0,V", 30);
        // osal_printk("Failed to parse GGA sentence, only %d fields matched\n", parsed);
    }

    if(ledFlag){
            uapi_gpio_toggle(CONFIG_BLINKY_PIN);
            uapi_gpio_set_val(CONFIG_BLINKY2_PIN,GPIO_LEVEL_LOW);
        }
        else{
            uapi_gpio_toggle(CONFIG_BLINKY2_PIN);
            osal_printk("警报！！！\r\n");
            uapi_gpio_set_val(CONFIG_BLINKY_PIN,GPIO_LEVEL_LOW);
        }
    // if(ledFlagRad){
    //         uapi_gpio_toggle(CONFIG_BLINKY2_PIN);
    //     }
    //     else{
    //         uapi_gpio_set_val(CONFIG_BLINKY2_PIN,GPIO_LEVEL_LOW);
    //     } 
    // // osal_printk("解析gngga:  %s\r\n\r\n",gngga);
    // osal_printk("纬度%s\r\n",gps_data.latitude);
    // osal_printk("经度%s\r\n",gps_data.longitude);
    // osal_printk("海拔%s\r\n",gps_data.altitude);
}

// 解析RMC句子
static void parse_rmc(const char *sentence) {
    char status;
    float speed, course;
    char date[12], time[12];
    char lat[16], lon[16];
    char lat_dir, lon_dir;
    
    // osal_printk("Raw RMC: %s\n", sentence);
    
    // 示例格式: $GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
    int parsed = sscanf(sentence, "$G%*CRMC,%11[^,],%c,%15[^,],%c,%15[^,],%c,%f,%f,%11[^,]",
                       time, &status, lat, &lat_dir, lon, &lon_dir, &speed, &course, date);
    
    // osal_printk("Parsed RMC fields: %d\n", parsed);
    
    if (parsed >= 9) {
        gps_data.status = status;
        gps_data.speed = speed;
        gps_data.course = course;
        
        // 解析日期
        size_t date_len = strlen(date);
        if (date_len == 6) {
            snprintf(gps_data.date, sizeof(gps_data.date), "%.2s-%.2s-%.2s", date, date+2, date+4);
            // osal_printk("RMC Date: %s\n", gps_data.date);
        }
        
        // 如果GGA没有提供时间，使用RMC的时间
        size_t time_len = strlen(time);
        if (strlen(gps_data.time) == 0 && time_len >= 6) {
            snprintf(gps_data.time, sizeof(gps_data.time), "%.2s:%.2s:%.2s", time, time+2, time+4);
            // osal_printk("RMC Time: %s\n", gps_data.time);
        }
        
        // osal_printk("RMC Status: %c, Speed: %.1f, Course: %.1f\n", status, speed, course);
    } else {
        // osal_printk("Failed to parse RMC sentence, only %d fields matched\n", parsed);
    }
}

// 解析VTG句子
static void parse_vtg(const char *sentence) {
    float speed_knots, speed_kmh;
    
    // osal_printk("Raw VTG: %s\n", sentence);
    
    // 示例格式: $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48
    int parsed = sscanf(sentence, "$G%*CVTG,%*f,T,%*f,M,%f,N,%f,K", &speed_knots, &speed_kmh);
    
    // osal_printk("Parsed VTG fields: %d\n", parsed);
    
    if (parsed >= 1) {
        gps_data.speed = speed_knots;
        // osal_printk("VTG Speed: %.1f knots (%.1f km/h)\n", speed_knots, speed_kmh);
    } else {
        // osal_printk("Failed to parse VTG sentence, only %d fields matched\n", parsed);
    }
}

// 解析GSA句子
static void parse_gsa(const char *sentence) {
    char mode;
    int fix_type;
    float pdop, hdop, vdop;
    
    // osal_printk("Raw GSA: %s\n", sentence);
    
    // 示例格式: $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
    int parsed = sscanf(sentence, "$G%*CGSA,%c,%d,%*[^,],%f,%f,%f", &mode, &fix_type, &pdop, &hdop, &vdop);
    
    // osal_printk("Parsed GSA fields: %d\n", parsed);
    
    if (parsed >= 5) {
        gps_data.mode = mode;
        gps_data.hdop = hdop;
        // osal_printk("GSA Mode: %c, Fix: %d, PDOP: %.1f, HDOP: %.1f, VDOP: %.1f\n",
        //           mode, fix_type, pdop, hdop, vdop);
    } else {
        // osal_printk("Failed to parse GSA sentence, only %d fields matched\n", parsed);
    }
}

// 处理完整的NMEA句子
static void process_nmea_sentence(const char *sentence) {
    
    if (strstr(sentence, "$GNGGA") == sentence || strstr(sentence, "$GPGGA") == sentence) {
        parse_gga(sentence);
    } else if (strstr(sentence, "$GNRMC") == sentence || strstr(sentence, "$GPRMC") == sentence) {
        parse_rmc(sentence);
    } else if (strstr(sentence, "$GNVTG") == sentence || strstr(sentence, "$GPVTG") == sentence) {
        parse_vtg(sentence);
    } else if (strstr(sentence, "$GNGSA") == sentence || strstr(sentence, "$GPGSA") == sentence) {
        parse_gsa(sentence);
    } else {
        // osal_printk("Unhandled NMEA sentence type: %s\n", sentence);
    }
    
    // 打印解析结果
    if (gps_data.status == 'A') {
        // osal_printk("GPS Data: Lat=%.6f, Lon=%.6f, Alt=%.1fm, Speed=%.1fkn, Course=%.1f°, "
        //            "Satellites=%d, HDOP=%.1f, Time=%s, Date=%s\n",
        //            gps_data.latitude, gps_data.longitude, gps_data.altitude,
        //            gps_data.speed, gps_data.course, gps_data.satellites,
        //            gps_data.hdop, gps_data.time, gps_data.date);
    } else {
        // osal_printk("GPS Data: No valid fix (Status: %c)\n", gps_data.status);
        // osal_printk("GPS Data: Lat=%.6f, Lon=%.6f, Alt=%.1fm, Speed=%.1fkn, Course=%.1f°, "
        //            "Satellites=%d, HDOP=%.1f\n",
        //            gps_data.latitude, gps_data.longitude, gps_data.altitude,
        //            gps_data.speed, gps_data.course, gps_data.satellites,
        //            gps_data.hdop);
    }
}
void app_gps_read_int_handler(const void *buffer, uint16_t length, bool error) {
    unused(error);
    if (buffer == NULL || length == 0) {
        osal_printk("uart%d int mode 传输非法数据!\r\n", CONFIG_GPS_BUS_ID);
        return;
    }
    if(openGpsFlag)
{
    const uint8_t *buff = (const uint8_t *)buffer;
    
    
    // 将新数据追加到缓冲区
    if (nmea_buffer_pos + length < sizeof(nmea_buffer)) {
        memcpy(nmea_buffer + nmea_buffer_pos, buff, length);
        nmea_buffer_pos += length;
        // osal_printk("Buffer position: %zu\n", nmea_buffer_pos);
    } else {
        // 缓冲区溢出，重置
        // osal_printk("NMEA buffer overflow! Resetting buffer.\n");
        nmea_buffer_pos = 0;
        return;
    }
    
    // 查找完整的NMEA句子
    char *start = nmea_buffer;
    while (1) {
        char *end = strchr(start, '\n');
        if (!end) break;
        
        // 确保句子以\r\n结尾
        if (end > start && *(end-1) == '\r') {
            *end = '\0';  // 终止字符串
            char *sentence_start = start;
            
            // 跳过可能的额外字符（如空格）
            while (*sentence_start && (*sentence_start == ' ' || *sentence_start == '\r')) {
                sentence_start++;
            }
            
            if (*sentence_start) {
                // osal_printk("Found complete NMEA sentence: %s\n", sentence_start);
                process_nmea_sentence(sentence_start);
            }
            
            // 移动剩余数据到缓冲区开头
            size_t remaining = nmea_buffer + nmea_buffer_pos - (end + 1);
            if (remaining > 0) {
                memmove(nmea_buffer, end + 1, remaining);
                nmea_buffer_pos = remaining;
                // osal_printk("Moved %zu bytes to buffer start\n", remaining);
            } else {
                nmea_buffer_pos = 0;
                // osal_printk("Buffer emptied\n");
            }
            start = nmea_buffer;
        } else {
            start = end + 1;
        }
    }

    }

}

static void *GPS_gather_task(const char *arg) {
    UNUSED(arg);
    osal_printk("Starting GPS task\n");
    

    uapi_pin_set_mode(CONFIG_BLINKY_PIN, PIN_MODE_0);
    uapi_gpio_set_dir(CONFIG_BLINKY_PIN, GPIO_DIRECTION_OUTPUT);
    uapi_gpio_set_val(CONFIG_BLINKY_PIN, GPIO_LEVEL_LOW);

    uapi_pin_set_mode(CONFIG_BLINKY2_PIN, PIN_MODE_0);
    uapi_gpio_set_dir(CONFIG_BLINKY2_PIN, GPIO_DIRECTION_OUTPUT);
    uapi_gpio_set_val(CONFIG_BLINKY2_PIN, GPIO_LEVEL_LOW);
    app_gps_init();
    osal_printk("Initialized GPS%d at %d baud\n", CONFIG_GPS_BUS_ID, GPS_BAUDRATE);
    
    osal_printk("Registering  GPS callback...\n");
    if (uapi_uart_register_rx_callback(CONFIG_GPS_BUS_ID, UART_RX_CONDITION_FULL_OR_IDLE,
                                      GPS_TRANSFER_SIZE, app_gps_read_int_handler) == ERRCODE_SUCC) {
        osal_printk("GPS %d callback registered successfully\n", CONFIG_GPS_BUS_ID);
    } else {
        osal_printk("Failed to register GPS callback\n");
    }
    
    while (1) {
        osal_printk("GPS task running...\n");
        osal_msleep(GPS_TASK_DURATION_MS);

        // (void)uapi_watchdog_kick();
        // if (uapi_uart_read(CONFIG_GPS_BUS_ID, g_app_uart_rx_buff, GPS_TRANSFER_SIZE, 0) > 0) {
        //     osal_printk("uart%d poll mode receive succ!, g_app_uart_rx_buff = %s\r\n", CONFIG_GPS_BUS_ID,
        //                 g_app_uart_rx_buff);
        // }
        
    }
    
    return NULL;
}

void GPS_gather_entry(void) {
    osThreadAttr_t attr;

    attr.name = "GPSTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = GPS_TASK_STACK_SIZE;
    attr.priority = GPS_TASK_PRIO;

    if (osThreadNew((osThreadFunc_t)GPS_gather_task, NULL, &attr) == NULL) {
        osal_printk("Failed to create GPS task.\n");
    }
}

// app_run(GPS_gather_entry);